
# 2008/11/10 updated the log to use the name
# 2008/11/10 updated the logs to be saved at logs folder
# 2008/11/10 updated the robotlog to run multiple robots


#This class Deals with creating path Logs which the robot has transversed 
#The Opengl livemapper also uses the logs created in this class
class RobotLog
 
 #Uses the robot class as the constructor
 def initialize(robotclass,opengl)
  @t_old=0 # a variable to store the value of the old time, used with position_log
  @t_old_las=0 # for the sick laser scanner
  @robotclass = robotclass 
  @ro_opengl=opengl #tells the class if you are using an opengl interface
  @interval=0.5
  @logflag=0 #prevent recall of logged 
  @turnlog=0# shall this program long fsonars, and rsonars when turning?
  
  @logscan=1 #turn of laser scanner
  @laserf=3
 end  
   
 #Clears all the log files
 def reset_logs
  open("../../logs/InsLog_#{@robotclass.robotname}.txt","w") do |file|
  end
  
if(@ro_opengl==true)
  open("../../mapdata/geomap_#{@robotclass.robotname}.dat","w") do |file|
  end
end
  
 end
 
 #start logging
def startlog()
  Thread.new{
    Thread.current["name"]="log_#{@robotclass.robotname}_startlog"
    @logging=true
    if(@robotclass.sen[0]==nil)
      raise "No data to recieve, use the recievestart() to recieve the data, first"
    end
    
    while (@logging==true )
      sleep(0.1) # required to synchronize since the begining roll pitch yaw is 0
      self.position_log()
    end
  }
end

#stop the log
def stoplog()
  @logging=false if @logging==true
end

 
 #This function creates a log with the X,Y,Z Position from the INS Sensor
 #The output file is named "InsLog.txt" And is Logged in the following format
 #<Global Time>,<X POS>,<Y POS>,<Z POS>,<PITCH>,<ROW>,<YAW>,updates in 1 second intervals
 def position_log()
   
   if(@logflag==0)
    @logflag=1 #log is running
    sen=@robotclass.sen
    status=@robotclass.status
    fsonar=@robotclass.fSONARSEN
    rsonar=@robotclass.rSONARSEN
    rs=@robotclass.rangescan

    interval=@interval
    moveflag=@robotclass.moveflag

 
   #begin logging the data  (NEW:IMPROVED)
    # @robotclass.recieve #call the recieve function
     ta1=@robotclass.getinsinfo(sen)
    # rs=@robotclass.getrangescanner(sen)
     name=@robotclass.robotname
     out2=""
     out2<<status[0]<<","<<name<<","<<ta1[0]<<"," <<ta1[1]<<","<<ta1[2]<<","<<ta1[3]<<","<<ta1[4]<<","<<ta1[5]
    
   #log the information at an interval difference of about interval seconds
    if out2[0..out2.index(",")-1].to_f - @t_old >interval
       @t_old=out2[0..out2.index(",")-1].to_f
      templastime=out2[0..out2.index(",")-1].to_f
     #prepare the fsonar info
      outfsonar=""   
      outfsonar<<"FSONAR,"<<status[0]<<","<<name<<","<<fsonar.join(",")
     
      #prepare the rsonar info
      outrsonar=""
      outrsonar<<"RSONAR,"<<status[0]<<","<<name<<","<<rsonar.join(",")
      
     #add the Range scanner infomation
     if(@logscan==1 and templastime - @t_old_las > interval*@laserf)
      outrangescan=""
      outrangescan<<"RANGESCAN,"<<status[0]<<","<<name<<","<<rs.join(",")
     end      
      
      #if turnlog=0 then, re-write out's time to -1
       if(@turnlog==0)
        if(moveflag!="forward" and moveflag!="")
          outfsonar=""   
          outfsonar<<"FSONAR,-1"<<","<<name<<","<<fsonar.join(",")  
          outrsonar=""  
          outrsonar<<"RSONAR,-1"<<","<<name<<","<<rsonar.join(",") 
       end
  
       if(moveflag!="forward" or  templastime - @t_old_las < interval*@laserf )
          outrangescan=""
          #outrangescan<<"RANGESCAN,-1,"<<name<<","<<rs.join(",")
          outrangescan<<"RANGESCAN,-1,"<<name<<","
       end
       if(templastime - @t_old_las >= interval*@laserf)
           @t_old_las=out2[0..out2.index(",")-1].to_f
       end
      end 
  
     #log the file in the InsLog 
     open("../../logs/InsLog_#{@robotclass.robotname}.txt","a") do |file|
            file.puts(out2)
          end
          
      #log the file to be used with the opengl interface
       outgl="INS,"<<out2
   
  
       if(@ro_opengl==true ) #only log 
         open("../../mapdata/geomap_#{@robotclass.robotname}.dat","a") do |file|
           file.puts(outgl)
           file.puts(outfsonar)
           file.puts(outrsonar)
           file.puts(outrangescan) if @logscan==1 
        end
      end 
      
       
      end
  
     @t_old=out2[0..out2.index(",")-1].to_f if @t_old==0 and out2!=nil
    
   @logflag=0 # free flag
   end #end if logflag
      
  end
 
end


